﻿using System;
using SunXNA.CameraManager.BaseCamera;
using Microsoft.Xna.Framework;
using System.Diagnostics;

using Microsoft.Xna.Framework.Input;

namespace Cameras
{
    class CFreeCam : CBaseCamera
    {
        #region Public Types
        #endregion

        #region Private Member Variables

        private const float m_rMovementSpeed = 50.0f;
        private const float m_rRotationSpeed = MathHelper.PiOver2; //MathHelper.ToRadians( 20.0f );

        private float m_rYaw;
        private float m_rPitch;

        private Vector3 m_v3Position;
        private Vector3 m_v3Look;
        private Vector3 m_v3Up;
        private Vector3 m_v3Right;

        #endregion

        #region Public Accessors
        #endregion

        #region Public Member Functions

        public CFreeCam()
        {
            m_v3Position = Vector3.Zero;
            m_v3Look = Vector3.Forward;
            m_v3Up = Vector3.Up;
            m_v3Right = Vector3.Right;

            m_rYaw = 0.0f;
            m_rPitch = 0.0f;
        }

        public override void Step(GameTime gameTime)
        {
            base.Step(gameTime);

            GamePadState gamePadState = GamePad.GetState(PlayerIndex.One);

            float rForward = 0.0f;
            float rRight = 0.0f;
            float rUp = 0.0f;
            float rYaw = 0.0f;
            float rPitch = 0.0f;

            if (gamePadState.IsConnected)
            {
                rForward = gamePadState.ThumbSticks.Left.Y;
                rRight = gamePadState.ThumbSticks.Left.X;
                rUp = (gamePadState.Triggers.Right - gamePadState.Triggers.Left);

                rYaw = -gamePadState.ThumbSticks.Right.X;
                rPitch = gamePadState.ThumbSticks.Right.Y;
            }
            else
            {
                KeyboardState keyboardState = Keyboard.GetState();
                MouseState mouseState = Mouse.GetState();

                rForward = keyboardState.IsKeyDown(Keys.W) ? 1.0f : keyboardState.IsKeyDown(Keys.S) ? -1.0f : 0.0f;
                rRight = keyboardState.IsKeyDown(Keys.D) ? 1.0f : keyboardState.IsKeyDown(Keys.A) ? -1.0f : 0.0f;
                rUp = keyboardState.IsKeyDown(Keys.Q) ? 1.0f : keyboardState.IsKeyDown(Keys.E) ? -1.0f : 0.0f;

                rYaw = keyboardState.IsKeyDown(Keys.Left) ? 1.0f : keyboardState.IsKeyDown(Keys.Right) ? -1.0f : 0.0f;
                rPitch = keyboardState.IsKeyDown(Keys.Up) ? 1.0f : keyboardState.IsKeyDown(Keys.Down) ? -1.0f : 0.0f;
            }

            float rForwardMovement = rForward * (m_rMovementSpeed * (float)gameTime.ElapsedGameTime.TotalSeconds);
            float rRightMovement = rRight * (m_rMovementSpeed * (float)gameTime.ElapsedGameTime.TotalSeconds);
            float rUpMovement = rUp * (m_rMovementSpeed * (float)gameTime.ElapsedGameTime.TotalSeconds);

            m_v3Position += rForwardMovement * m_v3Look;
            m_v3Position += rRightMovement * m_v3Right;
            m_v3Position += rUpMovement * m_v3Up;

            m_rYaw += rYaw * (m_rRotationSpeed * (float)gameTime.ElapsedGameTime.TotalSeconds);
            m_rPitch -= rPitch * (m_rRotationSpeed * (float)gameTime.ElapsedGameTime.TotalSeconds);

            m_v3Look = Vector3.Forward;
            m_v3Up = Vector3.Up;
            m_v3Right = Vector3.Right;

            Matrix m4YawMatrix = Matrix.CreateFromAxisAngle(m_v3Up, m_rYaw);
            m_v3Look = Vector3.Transform(m_v3Look, m4YawMatrix);
            m_v3Right = Vector3.Transform(m_v3Right, m4YawMatrix);

            Matrix m4PitchMatrix = Matrix.CreateFromAxisAngle(m_v3Right, m_rPitch);
            m_v3Look = Vector3.Transform(m_v3Look, m4PitchMatrix);
            m_v3Up = Vector3.Transform(m_v3Up, m4PitchMatrix);

            m_m4CurrentViewMatrix = Matrix.CreateLookAt(m_v3Position, m_v3Position + m_v3Look, m_v3Up);
        }

        #endregion
    }
}
